import rospy
from sensor_msgs.msg import LaserScan
import numpy as np


class LaserFilterNode:
    def __init__(self):
        rospy.Subscriber('/scan', LaserScan, self.laserScanCallback)
        self.pub = rospy.Publisher('/scan_filtered', LaserScan, queue_size=1)

    def laserScanCallback(self, msg):
        filtered_scan = LaserScan()
        filtered_scan.header = msg.header
        filtered_scan.angle_min = msg.angle_min
        filtered_scan.angle_max = msg.angle_max
        filtered_scan.angle_increment = msg.angle_increment
        filtered_scan.time_increment = msg.time_increment
        filtered_scan.scan_time = msg.scan_time
        filtered_scan.range_min = msg.range_min
        filtered_scan.range_max = msg.range_max
        filtered_scan.ranges = msg.ranges.copy()

        # Filter the scan to only include ranges between 0.25 m and 2 m from the robot
        filtered_scan.ranges[filtered_scan.ranges < 0.25] = np.nan
        filtered_scan.ranges[filtered_scan.ranges > 2] = np.nan

        self.pub.publish(filtered_scan)


if __name__ == '__main__':
    rospy.init_node('laser_filter')
    node = LaserFilterNode()
    rospy.spin()
